#include "ele.h"
#include "csbmath.h"

/*********************| static functions |*********************/
static void do_rotate(float* x, float* y, float rotate)
{
	//1.取负，获得360°内的数学库角度。计算特殊角度
	rotate = CSB_fmod(-rotate, 360.0f);
	if (!(rotate)) return;
	float tx = *x;
	float ty = *y;
	if (error_is_true(CSB_fmod(rotate , 180), 0.01f)) {
		*x = -tx;
		*y = -ty;
		return;
	}
	if (error_is_true(CSB_fmod(rotate , 90.0f),0.01f)) {
		if (CSB_fabs(rotate) > 90.0f) {
			// 270 绝对值缩放到90以获得象限的趋势
			rotate = (rotate < 0.0f) ? (360.0f + rotate) : (360.0f - rotate);
		}
		if (rotate < 0) {
			// 象限加1运算
			*y = -*x;
			*x = ty;
		} else {
			// 象限减一运算
			*x = -*y;
			*y = tx;
		}
		return;
	}
	//2.处理非特殊角度。获取原始角度的弧度数,加上angle获得旋转后的点的角度 
	float ora = M_PI_2;
	if (tx) {
		ora = CSB_atan(ty / tx);
		if (tx < 0)
			ora += M_PI;

	} else {
		if (ty < 0)
			ora = -M_PI_2;
	}
	ora += CSB_DEG2RAD(rotate);
	// 将目标弧度转化成(-180 - 180之间的角度)角度
	float tmpa = CSB_RAD2DEG(ora);
	tmpa = CSB_angle_normalize_180(tmpa);
	if (error_is_true(CSB_fmod(tmpa, 180.0f) , 0.01f)) {
		*x = CSB_sqrt((tx) * (tx) + (ty) * (ty));
		*y = 0;
		if (tmpa)
			*x = -*x;
	} else if (error_is_true(CSB_fmod(tmpa , 90.0f) , 0.01f)) {
		*y = CSB_sqrt((tx) * (tx) + (ty) * (ty));
		*x = 0;
		if (tmpa < 0)
			*y = -*y;
	} else {
		float tanora = CSB_fabs(CSB_tan(ora));
		ora = CSB_sqrt(((tx) * (tx) + (ty) * (ty)) / (1 + tanora * tanora));
		tx = ora;
		ty = ora * tanora;
		if (tmpa < -90)  {
			*y = -ty;
			*x = -tx;
		} else if (tmpa < 0) {
			*x = tx;
			*y = -ty;
		} else if (tmpa > 90) {
			*x = -tx;
			*y = ty;
		} else {
			*x = tx;
			*y = ty;
		}
	}
}

/*********************| extern func |*********************/
void elebase_coll_CP(const elebase_t* pele, const convex_polygon_t* oriCP, convex_polygon_t* out)
{
	csberrno = CSBERR_NOERR;
	CSB_NO_ERR_VRET(pele && oriCP && out && out->points && oriCP->points, CSBERR_PARAM);
	// rotate, scale , move
	float tmpx=0, tmpy = 0;
	uint32_t i = 0;
	while (i < oriCP->pnum) {
		tmpx = oriCP->points[i].x * pele->scale.xs;
		tmpy = oriCP->points[i].y * pele->scale.ys;
		do_rotate(&tmpx, &tmpy, pele->rotate);
		out->points[i].x = tmpx + pele->pos.x;
		out->points[i].y = -tmpy + pele->pos.y;
		++i;
	}
	out->pnum = oriCP->pnum;
}

float rotate_p2p(const pos_t* ori, const pos_t* target)
{
	csberrno = CSBERR_NOERR;
	CSB_NO_ERR_RET(ori && target, CSBERR_PARAM, 0);
	float a = (target->y - ori->y);
	float b = (target->x - ori->x);
	CSB_YES_RET(0 == a && 0 == b, 0);
	float t = 90.0f;
	if (0.0f != b) {
		t =  CSB_RAD2DEG(CSB_atan(a/b));
	}
	if (t < 0) {
		if (a > 0)
			t += 180.0f;
	} else if (t > 0) {
		if (a < 0)
			t += 180.0f;
	} else if (a < 0) {
		t += 180.0f;
	}
	return t;
}

